/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

#include "LOS_TM.h"

FIL file_DTM; //延时遥测专用

char FileName_SAT0[11] = "1:SAT0.BIN";
char FileName_SAT1[11] = "1:SAT1.BIN";
char FileName_SAT2[11] = "1:SAT2.BIN";
char FileName_SAT3[11] = "1:SAT3.BIN";
char FileName_ZGK0[11] = "1:ZGK0.BIN";
char FileName_ZGK1[11] = "1:ZGK1.BIN";
char FileName_GPS0[11] = "1:GPS0.BIN";
char FileName_UV10[11] = "1:UV10.BIN";
char FileName_PDB0[11] = "1:PDB0.BIN";
char FileName_TCB0[11] = "1:TCB0.BIN";
char FileName_CAM0[11] = "1:CAM0.BIN";
char FileName_NVE0[11] = "1:NVE0.BIN";
char FileName_NVE1[11] = "1:NVE1.BIN";
char FileName_SPT0[11] = "1:SPT0.BIN";
char FileName_CMS0[11] = "1:CMS0.BIN";
char FileName_FAM0[11] = "1:FAM0.BIN";

extern uint8_t FotonAmur_Data[128];
extern uint8_t Pack2Amur[128];

/** @fn void LOS_TMTC_TMPack(uint8_t nID, uint8_t type, uint8_t* pack)
*   @brief 对地遥测组包函数
*   @param[in] nID 需要组包的单机编号
*   @param[in] type 遥测包类型编号
*   @param[in] pack 遥测包内容:固定长度为218字节
*   @note 根据遥测协议，选择相应单机进行组包，该函数会自行添加7字节包头
*/
void LOS_TMTC_TMPack(uint8_t nID, uint8_t type, uint8_t *pack)
{
    //先添加7字节包头
    LOS_TMTC_TMHead(pack, nID, type, LOS_TMTC_GetTMPackSize(nID, type));

    switch(nID)
    {
        case nID_OBC:
        {
            LOS_TM_PackOBC(&pack[7], type);
        }
        break;

        case nID_AOCS:
        {
            LOS_TM_PackAOCS(&pack[7], type);
        }
        break;
        
        case nID_TTCBoard:
        {
            LOS_TM_PackTMTC(&pack[7], type);
        }
        break;

        case nID_BaseBand:
        {
            LOS_TM_PackDT(&pack[7], type);
        }
        break;

        case nID_PD:
        {
            LOS_TM_PackPD(&pack[7], type);
        }
        break;

        case nID_TC:
        {
            LOS_TM_PackTC(&pack[7], type);
        }
        break;

        case nID_CIOMPCam:
        {
            LOS_TM_PackCAM(&pack[7], type);
        }
        break;

        case nID_NaviEnhance:
        {
            LOS_TM_PackNE(&pack[7], type);
        }
        break;

        case nID_Spacetime:
        {
            LOS_TM_PackST(&pack[7], type);
        }
        break;

        case nID_CMOS:
        {
            LOS_TM_PackCMOS(&pack[7], type);
        }
        break;

        case nID_FotonAmur:
        {
            LOS_TM_PackAmur(&pack[7], type);
        }
        break;

        case nID_StarSensorY:
        {
            LOS_TM_PackSSY(&pack[7], type);
        }
        break;

        case nID_StarSensorZ:
        {
            LOS_TM_PackSSZ(&pack[7], type);
        }
        break;

        case nID_GNSS:
        {
            LOS_TM_PackGNSS(&pack[7], type);
        }
        break;

        default:
        {

        }
    }
}

/** @fn void LOS_TMTC_TMHead(uint8_t* tmmsg)
*   @brief 对地遥测添加包头函数
*   @param[in] tmmsg 遥测包内容:固定长度为208字节
*   @param[in] nID 区分不同单机遥测
*   @param[in] rec 区分同一单机的不同包
*   @note 添加一个六字节包主导头和一个一字节副导头
*/
void LOS_TMTC_TMHead(uint8_t* tmmsg, uint8_t nID, uint8_t rec, uint16_t len)
{
    uint16_t twobytes = 0;       //头两个字节，随便取的变量名（doge）
    uint16_t nID_buf;
    static uint16_t packcnt = 0; //包计数器
    nID_buf = nID;
    twobytes |= 0b000 << 13U;    //版本号：000
    twobytes |= 0 << 12U;        //标志类型，遥测：0
    twobytes |= 1 << 11U;        //有副导头
    twobytes |= 0b000 << 8U;     //应用过程标志：标准应用000
    twobytes |= nID_buf;         //应用过程标志：设备nID
    memcpy(tmmsg, &twobytes, 2);
    twobytes = 0;
    twobytes = packcnt | (0b11 << 14);
    memcpy(&tmmsg[2], &twobytes, 2);
    twobytes = len;              //去掉包主导头和副导头后的数据域长度
    memcpy(&tmmsg[4], &twobytes, 2);
    tmmsg[6] = rec;
    packcnt++;
    packcnt = packcnt % 16384;   //模16384
}


/** @fn void LOS_TMTC_PackOBC(uint8_t* tmmsg, uint8_t type)
*   @brief 卫星平台遥测包组包函数
*   @param[in] tmmsg 遥测包内容:固定长度为211字节(现在使用了201)
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackOBC(uint8_t* tmmsg, uint8_t type)
{
    float fbuf = 0;
    uint32_t ubuf = 0;
    int32_t sbuf = 0;
    uint8_t sumcnt = 0;
    uint16_t u16buf = 0;
    switch(type)
    {
        /* 基础遥测00包 SAT-S0 */
        case 0:
        {
            tmmsg[0] = 0xF0;
            tmmsg[1] = 0;
            memcpy(&tmmsg[2], &LOS_Time_SecFromBoot, 4);
            memcpy(&tmmsg[6], &LOS_Time_MilSec, 2);
            memcpy(&tmmsg[8], &LOS_Time_Sec, 4);
            memcpy(&tmmsg[12], STA_EPS.SWITCH_PD_O, 3);
            memcpy(&tmmsg[15], STA_EPS.SWITCH_TC_O, 3);
            memcpy(&tmmsg[18], &STA_EPS.Vol_12V, 2);
            memcpy(&tmmsg[20], &STA_EPS.Vol_5V, 2);
            memcpy(&tmmsg[22], &STA_EPS.Vol_3V3, 2);
            memcpy(&tmmsg[24], &STA_EPS.Cur_12V, 2);
            memcpy(&tmmsg[26], &STA_EPS.Cur_5V, 2);
            memcpy(&tmmsg[28], &STA_EPS.Cur_3V3, 2);
            memcpy(&tmmsg[30], STA_EPS.Vol_SA, 6);
            memcpy(&tmmsg[36], STA_EPS.Vol_MPPT, 6);
            memcpy(&tmmsg[42], STA_EPS.Cur_MPPT, 6);
            memcpy(&tmmsg[48], STA_EPS.Vol_Charger, 4);
            memcpy(&tmmsg[52], STA_EPS.Cur_Charger, 4);
            memcpy(&tmmsg[56], &LOS_TC_READ, 2);
            memcpy(&tmmsg[58], &LOS_TC_EX, 2);   
            memcpy(&tmmsg[60], &LOS_TC_LASTCMD, 4);
            memcpy(&tmmsg[64], &LOS_TC_FirstDelayCMD, 4);
            memcpy(&tmmsg[68], &LOS_TC_FirstDelayTime, 4);
            tmmsg[72] = MAX14830_SELFSTA1[1];
            tmmsg[73] = MAX14830_SELFSTA2[1];
            memcpy(&tmmsg[74], &STA_EPS.PD_CMDCNT, 2);
            memcpy(&tmmsg[76], &STA_EPS.TC_CMDCNT, 2);
            tmmsg[78] = LOS_TESTM_FLAG;
            memcpy(&tmmsg[79], &LOS_TESTM_LEFT, 4);
            memcpy(&tmmsg[83], &MODE_AOCS.mode, 2);
            memcpy(&tmmsg[85], &AOCS_Enter_Times, 4);
            memcpy(&tmmsg[89], &LOS_UPLOAD_FileSize, 4);
            memcpy(&tmmsg[93],&STA_TMTC.FTP_FileSize, 4);
            tmmsg[97] = LOS_UPLOAD_TC2OBC;
            memcpy(&tmmsg[98], &LOS_UPLOAD_FileReceived, 4);
            tmmsg[102] = LOS_UPLOAD_FileNum;
            tmmsg[103] = STA_TMTC.FTP_FileNum;
            memcpy(&tmmsg[104],&LOS_UPFILE_SIZE,4);
            memcpy(&tmmsg[108], &LOS_SD2FLASH_SIZE, 4);
            memcpy(&tmmsg[112], &CRC32_SD_SAVE, 4);
            memcpy(&tmmsg[116],&CRC32_TC_TRAN,4);
            memcpy(&tmmsg[120], &CRC32_SD_OFFSET, 4);
            memcpy(&tmmsg[124], &CRC32_SD_OBCCAL, 4);
            //对轨道系欧拉角
            //sbuf = aocs_g_output_struct.Ang_b2o[0]*10000*aocs_d_r2d;
            memcpy(&tmmsg[128], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Ang_b2o[1]*10000*aocs_d_r2d;
            memcpy(&tmmsg[132], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Ang_b2o[2]*10000*aocs_d_r2d;
            memcpy(&tmmsg[136], &sbuf, 4);
            //相对本体系角速度
            //sbuf = aocs_g_output_struct.W_b[0]*10000*aocs_d_r2d;
            memcpy(&tmmsg[140], &sbuf, 4);
            //sbuf = aocs_g_output_struct.W_b[1]*10000*aocs_d_r2d;
            memcpy(&tmmsg[144], &sbuf, 4);
            //sbuf = aocs_g_output_struct.W_b[2]*10000*aocs_d_r2d;
            memcpy(&tmmsg[148], &sbuf, 4);
            //解算磁场
            //sbuf = aocs_g_output_struct.Mag_b[0]*100000000;
            memcpy(&tmmsg[152], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Mag_b[1]*100000000;
            memcpy(&tmmsg[156], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Mag_b[2]*100000000;
            memcpy(&tmmsg[160], &sbuf, 4);
            //GNSS轨道六根数
            memcpy(&tmmsg[164], STA_GNSS.orbit_o, 24);
            tmmsg[188] = LOS_FLASH_ID;
            tmmsg[189] = LOS_CPU;
            memcpy(&tmmsg[190], &DWDRST_CNT, 4);
            memcpy(&tmmsg[194],&STA_EPS.Temp_3V3, 1);
            memcpy(&tmmsg[195],&STA_EPS.Temp_5V, 1);
            memcpy(&tmmsg[196],&STA_EPS.Temp_12V, 1);
            memcpy(&tmmsg[197],STA_EPS.Temp_MPPT, 3);
            tmmsg[200] = 0xCB;
            break;
        }

        /* 基础遥测B包 */
        case 1:
        {
            tmmsg[0] = 0xF0;
            tmmsg[1] = 1;
            memcpy(&tmmsg[2], &LOS_Time_SecFromBoot, 4);
            memcpy(&tmmsg[6], &LOS_Time_MilSec, 2);
            memcpy(&tmmsg[8], &LOS_Time_Sec, 4);
            memcpy(&tmmsg[12], &STA_FW[FW_X].TMCNT, 4);
            memcpy(&tmmsg[16], &STA_FW[FW_Y].TMCNT, 4);
            memcpy(&tmmsg[20], &STA_FW[FW_Z].TMCNT, 4);
            memcpy(&tmmsg[24], &STA_FW[FW_S].TMCNT, 4);
            memcpy(&tmmsg[28], &STA_SSY.TMCNT, 4);
            memcpy(&tmmsg[32], &STA_SSZ.TMCNT, 4);
            memcpy(&tmmsg[36], &STA_SUNS.TMCNT, 4);
            memcpy(&tmmsg[40], &STA_YH50.TMCNT, 4);
            memcpy(&tmmsg[44], &STA_MAGME.TMCNT, 4);
            memcpy(&tmmsg[48], &STA_GNSS.TMCNT_RADIO, 4);
            //配电开关电流
            memcpy(&tmmsg[52], &STA_EPS.Cur_PD25940[0], 48);
            //热控开关电流
            memcpy(&tmmsg[100], &STA_EPS.Cur_TC25940[0], 48);
            //热敏电阻
            memcpy(&tmmsg[148], &STA_EPS.Temp_Ther[0], 1);
            memcpy(&tmmsg[149], &STA_EPS.Temp_Ther[2], 1);
            memcpy(&tmmsg[150], &STA_EPS.Temp_Ther[5], 1);
            memcpy(&tmmsg[151], &STA_EPS.Temp_Ther[6], 1);
            memcpy(&tmmsg[152], &STA_EPS.Temp_Ther[9], 1);
            memcpy(&tmmsg[153], &STA_EPS.Temp_Ther[10], 1);
            memcpy(&tmmsg[154], &STA_EPS.Temp_Ther[13], 1);
            memcpy(&tmmsg[155], &STA_EPS.Temp_Ther[19], 1);
            memcpy(&tmmsg[156], &STA_EPS.Temp_Ther[20], 1);
            memcpy(&tmmsg[157], &STA_EPS.Temp_Ther[21], 1);
            memcpy(&tmmsg[158], &STA_EPS.Temp_Ther[22], 1);
            memcpy(&tmmsg[159], &STA_EPS.Temp_Ther[23], 1);
            memcpy(&tmmsg[160], &STA_EPS.Temp_Ther[24], 1);
            memcpy(&tmmsg[161], &STA_EPS.Temp_Ther[25], 1);
            memcpy(&tmmsg[162], &STA_EPS.Temp_Ther[26], 1);
            memcpy(&tmmsg[163], &STA_EPS.Temp_Ther[27], 1);
            memcpy(&tmmsg[164], &STA_EPS.Temp_Ther[28], 1);
            memcpy(&tmmsg[165], &STA_EPS.Temp_Ther[29], 1);
            memcpy(&tmmsg[166], &STA_EPS.Temp_Ther[30], 1);
            memcpy(&tmmsg[167], &STA_EPS.Temp_Ther[31], 1);
            //加热带
            memcpy(&tmmsg[168], &STA_EPS.Heater_UP[0], 1);
            memcpy(&tmmsg[169], &STA_EPS.Heater_LOW[0], 1);
            memcpy(&tmmsg[170], &STA_EPS.Heater_UP[1], 1);
            memcpy(&tmmsg[171], &STA_EPS.Heater_LOW[1], 1);
            memcpy(&tmmsg[172], &STA_EPS.Heater_UP[2], 1);
            memcpy(&tmmsg[173], &STA_EPS.Heater_LOW[2], 1);
            memcpy(&tmmsg[174], &STA_EPS.Heater_UP[3], 1);
            memcpy(&tmmsg[175], &STA_EPS.Heater_LOW[3], 1);
            memcpy(&tmmsg[176], &STA_EPS.Heater_UP[4], 1);
            memcpy(&tmmsg[177], &STA_EPS.Heater_LOW[4], 1);
            memcpy(&tmmsg[178], &STA_EPS.Heater_UP[5], 1);
            memcpy(&tmmsg[179], &STA_EPS.Heater_LOW[5], 1);
            memcpy(&tmmsg[180], &STA_EPS.Heater_UP[6], 1);
            memcpy(&tmmsg[181], &STA_EPS.Heater_LOW[6], 1);
            memcpy(&tmmsg[182], &STA_EPS.Heater_UP[7], 1);
            memcpy(&tmmsg[183], &STA_EPS.Heater_LOW[7], 1);
            memcpy(&tmmsg[184], &STA_EPS.Heater_UP[8], 1);
            memcpy(&tmmsg[185], &STA_EPS.Heater_LOW[8], 1);
            memcpy(&tmmsg[186], &STA_EPS.Heater_UP[9], 1);
            memcpy(&tmmsg[187], &STA_EPS.Heater_LOW[9], 1);
            memcpy(&tmmsg[188], &STA_EPS.Heater_UP[10], 1);
            memcpy(&tmmsg[189], &STA_EPS.Heater_LOW[10], 1);
            memcpy(&tmmsg[190], &STA_EPS.Heater_UP[11], 1);
            memcpy(&tmmsg[191], &STA_EPS.Heater_LOW[11], 1);
            memcpy(&tmmsg[192], &STA_EPS.Heater_UP[12], 1);
            memcpy(&tmmsg[193], &STA_EPS.Heater_LOW[12], 1);
            //配电错误指令计数
            memcpy(&tmmsg[194], &STA_EPS.PD_CMDERRCNT, 2);
            //热控错误指令计数
            memcpy(&tmmsg[196], &STA_EPS.TC_CMDERRCNT, 2);
            //配电遥测错误计数 
            tmmsg[198] = STA_EPS.PD_CRCERRCNT;
            //热控遥测错误计数
            tmmsg[199] = STA_EPS.TC_CRCERRCNT;
            //帧尾
            tmmsg[200] = 0xCB;
            break;
        }

        case 2:
        {
            tmmsg[0] = 0xF0;
            tmmsg[1] = 2;
            memcpy(&tmmsg[2], &LOS_Time_SecFromBoot, 4);
            memcpy(&tmmsg[6], &LOS_Time_MilSec, 2);
            memcpy(&tmmsg[8], &LOS_Time_Sec, 4);
            //飞轮转速
            sbuf = STA_FW[FW_X].TM_RPM * 10000;
            memcpy(&tmmsg[12],&sbuf,4);
            sbuf = STA_FW[FW_Y].TM_RPM * 10000;
            memcpy(&tmmsg[16],&sbuf,4);
            sbuf = STA_FW[FW_Z].TM_RPM * 10000;
            memcpy(&tmmsg[20],&sbuf,4);
            sbuf = STA_FW[FW_S].TM_RPM * 10000;
            memcpy(&tmmsg[24],&sbuf,4);
            //星敏Y
            memcpy(&tmmsg[28],&STA_SSY.q_o[0],4);
            memcpy(&tmmsg[32],&STA_SSY.q_o[1],4);
            memcpy(&tmmsg[36],&STA_SSY.q_o[2],4);
            memcpy(&tmmsg[40],&STA_SSY.q_o[3],4);
            memcpy(&tmmsg[44],&STA_SSY.angvel_o[0],4);
            memcpy(&tmmsg[48],&STA_SSY.angvel_o[1],4);
            memcpy(&tmmsg[52],&STA_SSY.angvel_o[2],4);
            tmmsg[56] = STA_SSY.StarNum;
            tmmsg[57] = STA_SSY.Flag_D;
            memcpy(&tmmsg[58],&STA_SSY.MODEB[0],1);
            //星敏Z
            memcpy(&tmmsg[59],&STA_SSZ.q_o[0],4);
            memcpy(&tmmsg[63],&STA_SSZ.q_o[1],4);
            memcpy(&tmmsg[67],&STA_SSZ.q_o[2],4);
            memcpy(&tmmsg[71],&STA_SSZ.q_o[3],4);
            memcpy(&tmmsg[75],&STA_SSZ.angvel_o[0],4);
            memcpy(&tmmsg[79],&STA_SSZ.angvel_o[1],4);
            memcpy(&tmmsg[83],&STA_SSZ.angvel_o[2],4);
            tmmsg[87] = STA_SSZ.StarNum;
            tmmsg[88] = STA_SSZ.Flag_D;
            memcpy(&tmmsg[89],&STA_SSZ.MODEB[0],1);
            //光纤陀螺
            sbuf = STA_YH50.x_gyro_deg * 10000;
            memcpy(&tmmsg[90],&sbuf,4);
            sbuf = STA_YH50.y_gyro_deg * 10000;
            memcpy(&tmmsg[94],&sbuf,4);
            sbuf = STA_YH50.z_gyro_deg * 10000;
            memcpy(&tmmsg[98],&sbuf,4);
            //太敏
            sbuf = STA_SUNS.alpha * 10000;
            memcpy(&tmmsg[102],&sbuf,4);
            sbuf = STA_SUNS.beta * 10000;
            memcpy(&tmmsg[106],&sbuf,4);
            //GPS位置速度
            memcpy(&tmmsg[110], &STA_GNSS.pos_vel_o[0], 24);
            //磁强计
            memcpy(&tmmsg[134], STA_MAGME.odata, 6);
            //MEMS陀螺
            sbuf = STA_MEMS.gyro_x * 10000;
            memcpy(&tmmsg[140], &sbuf, 4);
            sbuf = STA_MEMS.gyro_y * 10000;
            memcpy(&tmmsg[144], &sbuf, 4);
            sbuf = STA_MEMS.gyro_z * 10000;
            memcpy(&tmmsg[148], &sbuf, 4);
            sbuf = STA_MEMS.acc_x * 10000;
            memcpy(&tmmsg[152], &sbuf, 4);
            sbuf = STA_MEMS.acc_y * 10000;
            memcpy(&tmmsg[156], &sbuf, 4);
            sbuf = STA_MEMS.acc_z * 10000;
            memcpy(&tmmsg[160], &sbuf, 4);
            //对目标偏差四元数
            //sbuf = aocs_g_output_struct.Q_b2t[0] * 10000;
            memcpy(&tmmsg[164], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Q_b2t[1] * 10000;
            memcpy(&tmmsg[168], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Q_b2t[2] * 10000;
            memcpy(&tmmsg[172], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Q_b2t[3] * 10000;
            memcpy(&tmmsg[176], &sbuf, 4);
            //期望四元数
            //sbuf = aocs_g_output_struct.Q_t2i[0] * 10000;
            memcpy(&tmmsg[180], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Q_t2i[1] * 10000;
            memcpy(&tmmsg[184], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Q_t2i[2] * 10000;
            memcpy(&tmmsg[188], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Q_t2i[3] * 10000;
            memcpy(&tmmsg[192], &sbuf, 4);
            //待执行延时指令计数
            memcpy(&tmmsg[196], &LOS_TC_TaskNum, 4);
            tmmsg[200] = 0xCB;
            break;
        }

        case 3:
        {
            tmmsg[0] = 0xF0;
            tmmsg[1] = 3;
            memcpy(&tmmsg[2], &LOS_Time_SecFromBoot, 4);
            memcpy(&tmmsg[6], &LOS_Time_MilSec, 2);
            memcpy(&tmmsg[8], &LOS_Time_Sec, 4);
            //解算四元数
            //sbuf = aocs_g_output_struct.Q_b2i[0] * 10000;
            memcpy(&tmmsg[12], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Q_b2i[1] * 10000;
            memcpy(&tmmsg[16], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Q_b2i[2] * 10000;
            memcpy(&tmmsg[20], &sbuf, 4);
            //sbuf = aocs_g_output_struct.Q_b2i[3] * 10000;
            memcpy(&tmmsg[24], &sbuf, 4);
            //飞轮控制力矩
            //sbuf = aocs_g_output_struct.RW_Tcmd[0] * 1000000;
            memcpy(&tmmsg[28], &sbuf, 4);
            //sbuf = aocs_g_output_struct.RW_Tcmd[1] * 1000000;
            memcpy(&tmmsg[32], &sbuf, 4);
            //sbuf = aocs_g_output_struct.RW_Tcmd[2] * 1000000;
            memcpy(&tmmsg[36], &sbuf, 4);
            //sbuf = aocs_g_output_struct.RW_Tcmd[3] * 1000000;
            memcpy(&tmmsg[40], &sbuf, 4);
            //磁力矩器控制占空比
//            memcpy(&tmmsg[44], &aocs_g_output_struct.MGT_Mcmd[0], 1);
//            memcpy(&tmmsg[45], &aocs_g_output_struct.MGT_Mcmd[1], 1);
//            memcpy(&tmmsg[46], &aocs_g_output_struct.MGT_Mcmd[2], 1);
//            memcpy(&tmmsg[47], &aocs_g_output_struct.MGT_Mcmd[3], 1);
//            memcpy(&tmmsg[48], &aocs_g_output_struct.MGT_Mcmd[4], 1);
            //母线电压
            sbuf = STA_EPS.Mother_Vol;
            memcpy(&tmmsg[49], &sbuf, 4);
            //安时计
            sbuf = STA_EPS.AhMeter;
            memcpy(&tmmsg[53], &sbuf, 4);
            //软件版本号
            u16buf = SOFTVERSION;
            memcpy(&tmmsg[57], &u16buf, 2);
            //磁力矩器+Z控制占空比
            //memcpy(&tmmsg[59], &aocs_g_output_struct.MGT_Mcmd[5], 1);
            //上一次收到遥控指令的时间
            memcpy(&tmmsg[60], &LOS_TC_LASTCMDTIME, 4);
            //上注文件写入SD卡基址
            memcpy(&tmmsg[64], &LOS_UP_BASEADDR, 4);
            tmmsg[68] = FLASH_WHERE;
            tmmsg[69] = STA_DT.ENWDT;
            //小相机最近一张照片编号
            memcpy(&tmmsg[71], &STA_CMOS.lastpicid, 4);
            //小相机最近一张照片长度
            memcpy(&tmmsg[75], &STA_CMOS.lastpiclen, 4);
            //SD卡至测控剩余包数
            memcpy(&tmmsg[79], &LOS_CMOS_OBC2TCLEFT, 4);
            //姿轨控程序建议模式
            //memcpy(&tmmsg[83], &aocs_g_output_struct.Mode_Request, 2);
            //小相机转存SD卡的一些状态
            tmmsg[85] = LOS_CMOS_PICNUM;
            tmmsg[86] = LOS_CMOS_OBC2TM;
            tmmsg[87] = STA_CMOS.piccnt;
            tmmsg[88] = LOS_CMOS_NUMBUF;
            memcpy(&tmmsg[89], &CMOS_NOWSIZE, 4);
            tmmsg[93] = STA_CMOS.picid_TC;
            memcpy(&tmmsg[94], &STA_CMOS.readoffset, 4);
            tmmsg[98] = STA_CMOS.readsta;
            
            //姿控调试量
            //fbuf = aocs_g_output_struct.S_i[0];
            memcpy(&tmmsg[99], &fbuf, 4);
            //fbuf = aocs_g_output_struct.S_i[1];
            memcpy(&tmmsg[103], &fbuf, 4);
            //fbuf = aocs_g_output_struct.S_i[2];
            memcpy(&tmmsg[107], &fbuf, 4);
            //fbuf = aocs_g_output_struct.S_i[3];
            memcpy(&tmmsg[111], &fbuf, 4);
            //fbuf = aocs_g_output_struct.S_i[4];
            memcpy(&tmmsg[115], &fbuf, 4);
            //fbuf = aocs_g_output_struct.S_i[5];
            memcpy(&tmmsg[119], &fbuf, 4);
            //fbuf = aocs_g_output_struct.S_i[6];
            memcpy(&tmmsg[123], &fbuf, 4);
            //fbuf = aocs_g_output_struct.S_i[7];
            memcpy(&tmmsg[127], &fbuf, 4);
            //fbuf = aocs_g_output_struct.S_i[8];
            memcpy(&tmmsg[131], &fbuf, 4);
            //fbuf = aocs_g_output_struct.S_i[9];
            memcpy(&tmmsg[135], &fbuf, 4);

            //fbuf = aocs_g_output_struct.B_i[0];
            memcpy(&tmmsg[139], &fbuf, 4);
            //磁强计补偿后的值
            memcpy(&tmmsg[143], &STA_MAGME.AfterComp[0], 4);
            memcpy(&tmmsg[147], &STA_MAGME.AfterComp[1], 4);
            memcpy(&tmmsg[151], &STA_MAGME.AfterComp[2], 4);
            memcpy(&tmmsg[155], &STA_DT.FTP_FileSize, 4);
            tmmsg[159] = STA_DT.FTP_FileNum;
            tmmsg[160] = LOS_UPLOAD_DT2OBC;
            memcpy(&tmmsg[161],&LOS_UPFILE_SIZE,4);

            //上注目标经纬度地心距
            memcpy(&tmmsg[167], &LOS_Targetpos[0], 4);
            memcpy(&tmmsg[171], &LOS_Targetpos[1], 4);
            memcpy(&tmmsg[175], &LOS_Targetpos[2], 4);
            //上注目标四元数
            memcpy(&tmmsg[179], &LOS_Targetq[0], 4);
            memcpy(&tmmsg[183], &LOS_Targetq[1], 4);
            memcpy(&tmmsg[187], &LOS_Targetq[2], 4);
            memcpy(&tmmsg[191], &LOS_Targetq[3], 4);
            //整星角动量
            //sbuf = aocs_g_output_struct.H_total*1000000;
            memcpy(&tmmsg[195], &sbuf, 4);

            tmmsg[200] = 0xCB;
            break;
        }

        case 4:
        {
            tmmsg[0] = 0xF0;
            tmmsg[1] = 4;
            memcpy(&tmmsg[2], &LOS_Time_SecFromBoot, 4);
            memcpy(&tmmsg[6], &LOS_Time_MilSec, 2);
            memcpy(&tmmsg[8], &LOS_Time_Sec, 4);
            memcpy(&tmmsg[12], &FR_UTC, 4);
            tmmsg[16] = FR_UTC_SEL;
            memcpy(&tmmsg[17], &FR_TKANTEN, 4);
            tmmsg[21] = FR_TKANTEN_SEL;
            memcpy(&tmmsg[22], &FR_TKSAEN, 4);
            tmmsg[26] = FR_TKSAEN_SEL;
            memcpy(&tmmsg[27], &FR_DELAYEN, 4);
            tmmsg[31] = FR_DELAYEN_SEL;
            memcpy(&tmmsg[32], &FR_DELAYDUR, 4);
            tmmsg[36] = FR_DELAYDUR_SEL;
            memcpy(&tmmsg[37], FR_MATSSY, 36);
            tmmsg[73] = FR_MATSSY_SEL;
            memcpy(&tmmsg[74], FR_MATSSZ, 36);
            tmmsg[110] = FR_MATSSZ_SEL;
            memcpy(&tmmsg[111], FR_MATYH50, 36);
            tmmsg[147] = FR_MATYH50_SEL;
            memcpy(&tmmsg[148], FR_MATSUNS, 36);
            tmmsg[184] = FR_MATSUNS_SEL;
            memcpy(&tmmsg[185], FR_MGTMAX, 24);
            tmmsg[209] = FR_MGTMAX_SEL;
            tmmsg[210] = 0XCB;
            break;
        }

        case 5:
        {
            tmmsg[0] = 0xF0;
            tmmsg[1] = 5;
            memcpy(&tmmsg[2], &LOS_Time_SecFromBoot, 4);
            memcpy(&tmmsg[6], &LOS_Time_MilSec, 2);
            memcpy(&tmmsg[8], &LOS_Time_Sec, 4);
            memcpy(&tmmsg[12], FR_MATMAGME, 36);
            tmmsg[48] = FR_MATMAGME_SEL;
            memcpy(&tmmsg[49], FR_MQSSY, 16);
            tmmsg[65] = FR_MQSSY_SEL;
            memcpy(&tmmsg[66], FR_MQSSZ, 16);
            tmmsg[82] = FR_MQSSZ_SEL;
            //模式切换阈值
            memcpy(&tmmsg[83], FR_MCTR, 68);
            tmmsg[151] = FR_MCTR_SEL;
            memcpy(&tmmsg[152], &FR_OBC, 4);
            tmmsg[156] = FR_OBC_SEL;
            memcpy(&tmmsg[157], &FR_TKMAXCNT, 4);
            tmmsg[161] = FR_TKMAXCNT_SEL;
            memcpy(&tmmsg[162], &FR_TKDUR, 4);
            tmmsg[166] = FR_TKDUR_SEL;
            memcpy(&tmmsg[167], &FR_GPSAUTO, 4);
            tmmsg[171] = FR_GPSAUTO_SEL;
            memcpy(&tmmsg[172], &FR_MAGCOMP, 12);
            tmmsg[184] = FR_MAGCOMP_SEL;
            memcpy(&tmmsg[185], FR_DTAUTO, 8);
            tmmsg[193] = FR_DTAUTO_SEL;

            tmmsg[210] = 0XCB;
            break;
        }

        default:

    }

}

/** @fn void LOS_TM_PackAOCS(uint8_t* tmmsg, uint8_t type)
*   @brief 姿轨控分系统遥测包组包函数
*   @param[in] tmmsg 遥测包内容:固定长度为201字节,需要加入10字节时间信息
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackAOCS(uint8_t* tmmsg, uint8_t type)
{
    static float fbuf = 0;
    static int16_t i16buf = 0;
    switch(type)
    {
        case 0:
        {
            tmmsg[0] = 0xAA;
            tmmsg[1] = 0;
            memcpy(&tmmsg[2], &LOS_Time_SecFromBoot, 4);
            memcpy(&tmmsg[6], &LOS_Time_MilSec, 2);
            memcpy(&tmmsg[8], &LOS_Time_Sec, 4);
            memcpy(&tmmsg[12], &TMPACK_FWX[11], 15);
            memcpy(&tmmsg[27], &TMPACK_FWY[11], 15);
            memcpy(&tmmsg[42], &TMPACK_FWZ[11], 15);
            memcpy(&tmmsg[57], &TMPACK_FWS[11], 15);
            memcpy(&tmmsg[72], &TMPACK_SUNSBasic[10], 16);
            memcpy(&tmmsg[88], &TMPACK_YH50[10], 22);
            memcpy(&tmmsg[110], &TMPACK_GNSSRadio[10], 54);
            memcpy(&tmmsg[164], &TMPACK_MAGME[10], 7);
            //tmmsg[171] = aocs_g_orbsolver_struct.Orbit_Flag;
            //tmmsg[172] = aocs_g_orbsolver_struct.OP_Flag;
            //fbuf = aocs_g_pointpara_struct.W_t[0];
            memcpy(&tmmsg[173], &fbuf, 4);
            //fbuf = aocs_g_pointpara_struct.W_t[1];
            memcpy(&tmmsg[177], &fbuf, 4);
            //fbuf = aocs_g_pointpara_struct.W_t[2];
            memcpy(&tmmsg[181], &fbuf, 4);
            //i16buf = aocs_g_output_struct.Sign_backup;
            memcpy(&tmmsg[185], &i16buf, 2);
            //i16buf = aocs_g_output_struct.Sign_unload;
            memcpy(&tmmsg[187], &i16buf, 2);
            //i16buf = aocs_g_output_struct.Sun_Flag;
            memcpy(&tmmsg[189], &i16buf, 2);
            //i16buf = aocs_g_output_struct.Hbrake;
            memcpy(&tmmsg[191], &i16buf, 2);
            //fbuf = aocs_g_output_struct.Modetime;
            //memcpy(&tmmsg[193], &aocs_g_output_struct.Modetime, 4);
            tmmsg[198] = 0xCB;
            break;
        }
        
        case 1:
        {
            tmmsg[0] = 0xAA;
            tmmsg[1] = 1;
            memcpy(&tmmsg[2], &LOS_Time_SecFromBoot, 4);
            memcpy(&tmmsg[6], &LOS_Time_MilSec, 2);
            memcpy(&tmmsg[8], &LOS_Time_Sec, 4);
            memcpy(&tmmsg[12], &TMPACK_SSYShort[10], 62);
            memcpy(&tmmsg[74], &TMPACK_SSZShort[10], 62);
            //i16buf = aocs_g_health_struct.GNSS_FlagF;
            memcpy(&tmmsg[136], &i16buf, 2);
            //memcpy(&tmmsg[138], &aocs_g_health_struct.GNSS_Cnt, 4);
            //i16buf = aocs_g_health_struct.ST_FlagF[0];
            memcpy(&tmmsg[142], &i16buf, 2);
            //i16buf = aocs_g_health_struct.ST_FlagF[1];
            memcpy(&tmmsg[144], &i16buf, 2);
            //memcpy(&tmmsg[146], &aocs_g_health_struct.ST_Cnt[0], 4);
            //memcpy(&tmmsg[150], &aocs_g_health_struct.ST_Cnt[1], 4);
            //i16buf = aocs_g_health_struct.GYRO_FlagF[0];
            memcpy(&tmmsg[154], &i16buf, 2);
            //memcpy(&tmmsg[156], &aocs_g_health_struct.GYRO_Cnt[0], 4);
            //i16buf = aocs_g_health_struct.FSS_FlagF[0];
            memcpy(&tmmsg[160], &i16buf, 2);
            //memcpy(&tmmsg[162], &aocs_g_health_struct.FSS_Cnt[0], 4);
            //i16buf = aocs_g_health_struct.MGM_FlagF[0];
            memcpy(&tmmsg[166], &i16buf, 2);
            //memcpy(&tmmsg[168], &aocs_g_health_struct.FSS_Cnt[0], 4);
            //i16buf = aocs_g_health_struct.RW_FlagF[0];
            memcpy(&tmmsg[172], &i16buf, 2);
            //i16buf = aocs_g_health_struct.RW_FlagF[1];
            memcpy(&tmmsg[174], &i16buf, 2);
            //i16buf = aocs_g_health_struct.RW_FlagF[2];
            memcpy(&tmmsg[176], &i16buf, 2);
            //i16buf = aocs_g_health_struct.RW_FlagF[3];
            memcpy(&tmmsg[178], &i16buf, 2);
//            memcpy(&tmmsg[180], &aocs_g_health_struct.RW_Cnt[0], 4);
//            memcpy(&tmmsg[184], &aocs_g_health_struct.RW_Cnt[1], 4);
//            memcpy(&tmmsg[188], &aocs_g_health_struct.RW_Cnt[2], 4);
//            memcpy(&tmmsg[192], &aocs_g_health_struct.RW_Cnt[3], 4);
//            memcpy(&tmmsg[196], &aocs_g_health_struct.SafeModeTimeCnt, 4);
            tmmsg[200] = 0xCB;
            break;
        }

        default:
    }
}

/** @fn void LOS_TM_PackSSY(uint8_t* tmmsg, uint8_t type)
*   @brief 星敏感器Y组包函数
*   @param[in] tmmsg 遥测包内容
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackSSY(uint8_t* tmmsg, uint8_t type)
{
    switch(type)
    {
        /* 自检遥测回复包 */
        case 1:
        {
            tmmsg[0] = 0xB5;
            tmmsg[1] = 1;
            memcpy(&tmmsg[2], &TMPACK_SSYCheck[0], 15);
            tmmsg[17] = 0xCB;
            break;
        }

        /* 参数下传包 */
        case 2:
        {
            tmmsg[0] = 0xB5;
            tmmsg[1] = 2;
            memcpy(&tmmsg[2], &TMPACK_SSYArg[0], 112);
            tmmsg[114] = 0xCB;
            break;
        }

        /* 数据详包 */
        case 3:
        {
            tmmsg[0] = 0xB5;
            tmmsg[1] = 3;
            memcpy(&tmmsg[2], TMPACK_SSYLong, 471);
            tmmsg[473] = 0xCB;
            break;
        }

        default:

    }
}

/** @fn void LOS_TM_PackSSZ(uint8_t* tmmsg, uint8_t type)
*   @brief 星敏感器Z组包函数
*   @param[in] tmmsg 遥测包内容:固定长度为201字节,需要加入12字节包头
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackSSZ(uint8_t* tmmsg, uint8_t type)
{
    switch(type)
    {
        /* 自检遥测回复包 */
        case 1:
        {
            tmmsg[0] = 0xB6;
            tmmsg[1] = 1;
            memcpy(&tmmsg[2], &TMPACK_SSZCheck[0], 15);
            tmmsg[17] = 0xCB;
            break;
        }

        /* 参数下传包 */
        case 2:
        {
            tmmsg[0] = 0xB6;
            tmmsg[1] = 2;
            memcpy(&tmmsg[2], &TMPACK_SSZArg[0], 112);
            tmmsg[114] = 0xCB;
            break;
        }

        /* 数据详包 */
        case 3:
        {
            tmmsg[0] = 0xB6;
            tmmsg[1] = 3;
            memcpy(&tmmsg[2], TMPACK_SSZLong, 471);
            tmmsg[473] = 0xCB;
            break;
        }

        default:

    }
}

/** @fn void LOS_TM_PackGNSS(uint8_t* tmmsg, uint8_t type)
*   @brief GNSS组包函数
*   @param[in] tmmsg 遥测包内容:固定长度为201字节,需要加入12字节包头
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackGNSS(uint8_t* tmmsg, uint8_t type)
{
    switch(type)
    {
        case 0:
        {
            tmmsg[0] = 0xB8;
            tmmsg[1] = 0;
            memcpy(&tmmsg[2], &TMPACK_GNSSBasic[0], 161);
            tmmsg[163] = 0xCB;
            break;
        }

        default:
    }
}

/** @fn void LOS_TM_PackTMTC(uint8_t* tmmsg, uint8_t type)
*   @brief 测控板组包函数
*   @param[in] tmmsg 遥测包内容:固定长度为201字节,需要加入12字节包头
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackTMTC(uint8_t* tmmsg, uint8_t type)
{
    switch(type)
    {
        case 0:
        {
            memcpy(tmmsg, &TMPACK_TMTCBasic[0], 120);
            break;
        }

        default:
    }
}

/** @fn void LOS_TM_PackDT(uint8_t* tmmsg, uint8_t type)
*   @brief 数传组包函数
*   @param[in] tmmsg 遥测包内容
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackDT(uint8_t* tmmsg, uint8_t type)
{
    switch(type)
    {
        case 0:
        {
            memcpy(tmmsg, &TMPACK_DTBasic[0], 91);
            break;
        }

        default:
    }
}

/** @fn void LOS_TM_PackPD(uint8_t* tmmsg, uint8_t type)
*   @brief 配电扩展板组包函数
*   @param[in] tmmsg 遥测包内容:固定长度为201字节,需要加入12字节包头
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackPD(uint8_t* tmmsg, uint8_t type)
{
    switch(type)
    {
        case 0:
        {
            tmmsg[0] = 0x04;
            tmmsg[1] = 0;
            memcpy(&tmmsg[2], &TMPACK_PDB[0], 116);
            tmmsg[118] = 0xCB;
            break;
        }

        default:
    }
}

/** @fn void LOS_TM_PackTC(uint8_t* tmmsg, uint8_t type)
*   @brief 热控扩展板组包函数
*   @param[in] tmmsg 遥测包内容:固定长度为201字节,需要加入12字节包头
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackTC(uint8_t* tmmsg, uint8_t type)
{
    switch(type)
    {
        case 0:
        {
            tmmsg[0] = 0x05;
            tmmsg[1] = 0;
            memcpy(&tmmsg[2], &TMPACK_TCB[0], 132);
            tmmsg[134] = 0xCB;
            break;
        }

        default:
    }
}

/** @fn void LOS_TM_PackNE(uint8_t* tmmsg, uint8_t type)
*   @brief 导航增强组包函数
*   @param[in] tmmsg 遥测包内容
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackNE(uint8_t* tmmsg, uint8_t type)
{
    switch(type)
    {
        case 0:
        {
            tmmsg[0] = 0xA5;
            tmmsg[1] = 0;
            memcpy(&tmmsg[2], &navi_dabuf[0], 370);
            tmmsg[372] = 0xCB;
            break;
        }
        default:
    }
}

/** @fn void LOS_TM_PackCAM(uint8_t* tmmsg, uint8_t type)
*   @brief 高分相机组包函数
*   @param[in] tmmsg 遥测包内容:固定长度为201字节,需要加入12字节包头
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包,包括高分相机和存储板的遥测
*/
void LOS_TM_PackCAM(uint8_t* tmmsg, uint8_t type)
{
    switch(type)
    {
        case 0:
        {
            tmmsg[0] = 0xA9;
            tmmsg[1] = 0;
            memcpy(&tmmsg[2], &TMPACK_CIOMPCam[0], 45);
            memcpy(&tmmsg[47], &TMPACK_SATA[0], 16);
            tmmsg[63] = 0xCB;
            break;
        }

        default:
    }
}

/** @fn void LOS_TM_PackST(uint8_t* tmmsg, uint8_t type)
*   @brief 时空表组包函数
*   @param[in] tmmsg 遥测包内容:固定长度为201字节,需要加入12字节包头
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackST(uint8_t* tmmsg, uint8_t type)
{
    switch(type)
    {
        case 0:
        {
            tmmsg[0] = 0xA6;
            tmmsg[1] = 0;
            memcpy(&tmmsg[2], &SpaceTime_TMDATA[0], 42);
            tmmsg[200] = 0xCB;
            tmmsg[201] = 0xCB;
            tmmsg[202] = 0xCB;
            tmmsg[203] = 0xCB;
            tmmsg[204] = 0xCB;
            tmmsg[205] = 0xCB;
            tmmsg[206] = 0xCB;
            tmmsg[207] = 0xCB;
            tmmsg[208] = 0xCB;
            tmmsg[209] = 0xCB;
            tmmsg[210] = 0xCB;
            break;
        }

        default:
    }
}

/** @fn void LOS_TM_PackCMOS(uint8_t* tmmsg, uint8_t type)
*   @brief 小相机遥测包组包函数
*   @param[in] tmmsg 遥测包内容:固定长度为201字节,需要加入12字节包头
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackCMOS(uint8_t* tmmsg, uint8_t type)
{
    switch(type)
    {
        case 0:
        {
            tmmsg[0] = 0xA8;
            tmmsg[1] = 0;
            memcpy(&tmmsg[2], &LOS_Time_SecFromBoot, 4);
            memcpy(&tmmsg[6], &LOS_Time_MilSec, 2);
            memcpy(&tmmsg[8], &LOS_Time_Sec, 4);
            memcpy(&tmmsg[12], cmos_tmbuf, 136);
            tmmsg[148]=0xCB;
            break;
        }
    }
}

/** @fn void LOS_TM_PackAmur(uint8_t* tmmsg, uint8_t type)
*   @brief 光电传感器组包函数
*   @param[in] tmmsg 遥测包内容
*   @param[in] type 遥测包类型 
*   @note 根据遥测协议进行组包
*/
void LOS_TM_PackAmur(uint8_t* tmmsg, uint8_t type)
{
    for(int i = 0; i<128; i++)
    {
        tmmsg[i] = 0;
    }
    switch(type)
    {
        case 0:
        memcpy(tmmsg, FotonAmur_Data, 128);
        break;

        case 1:
        memcpy(tmmsg, Pack2Amur, 128);
        break;

        default:

    }

}

/** @fn void LOS_TMTC_GetTMPackSize(uint8_t nID, uint8_t type)
*   @brief 获取遥测包的长度
*   @param[in] nID 单机nID
*   @param[in] type 遥测包类型 
*   @note 不同遥测包的数据域长度在该函数中规定
*/
uint16_t LOS_TMTC_GetTMPackSize(uint8_t nID, uint8_t type)
{
    switch(nID)
    {
        case nID_OBC:
        {
            switch(type)
            {
                case 0:
                return 201;
                
                case 1:
                return 201;

                case 2:
                return 201;

                case 3:
                return 201;

                case 4:
                return 211;

                case 5:
                return 211;

                default:
                return 0;
            }
        }
        
        case nID_AOCS:
        {
            switch(type)
            {
                case 0:
                return 198;

                case 1:
                return 201;

                default:
                return 0;
            }
        }

        case nID_StarSensorY:
        {
            switch(type)
            {
                case 0:
                return 18;

                case 1:
                return 115;

                case 2:
                return 474;

                default:
                return 0;
            }
        }

        case nID_StarSensorZ:
        {
            switch(type)
            {
                case 0:
                return 18;

                case 1:
                return 115;

                case 2:
                return 474;

                default:
                return 0;
            }
        }

        case nID_GNSS:
        {
            switch(type)
            {
                case 0:
                return 164;

                default:
                return 0;
            }
        }

        case nID_TTCBoard:
        {
            switch(type)
            {
                case 0:
                return 120;

                default:
                return 0;
            }
        }

        case nID_BaseBand:
        {
            switch(type)
            {
                case 0:
                return 91;

                default:
                return 0;
            }
        }

        case nID_PD:
        {
            switch(type)
            {
                case 0:
                return 119;

                default:
                return 0;
            }
        }

        case nID_TC:
        {
            switch(type)
            {
                case 0:
                return 135;

                default:
                return 0;
            }
        }

        case nID_CIOMPCam:
        {
            switch(type)
            {
                case 0:
                return 64;

                default:
                return 0;
            }
        }

        case nID_NaviEnhance:
        {
            switch(type)
            {
                case 0:
                return 373;

                default:
                return 0;
            }
        }

        case nID_Spacetime:
        {
            switch(type)
            {
                case 0:
                return 201;

                default:
                return 0;
            }
        }

        case nID_CMOS:
        {
            switch(type)
            {
                case 0:
                return 149;

                default:
                return 0;
            }
        }

        case nID_FotonAmur:
        {
            return 128;
        }

        default:
        break;
    }
    return 0;
}

/* 慢遥测分配函数 */
void LOS_TM_SlowTMArbitrate(uint8_t packnum)
{
    static uint8_t packsel = 0;

    if (LOS_TM_PackNum[packnum] == 0)
    {
        switch(LOS_TM_GetPDPWRON())
        {
            case 0:  //存储板
            LOS_TM_ID[packnum] = nID_CIOMPCam;
            LOS_TM_Type[packnum] = 0; 
            break;

            case 1:  //数传基带
            LOS_TM_ID[packnum] = nID_BaseBand;
            LOS_TM_Type[packnum] = 0; 
            break;

            case 2:  //数传通道
            LOS_TM_ID[packnum] = nID_BaseBand;
            LOS_TM_Type[packnum] = 0; 
            break;

            case 7:  //GNSS板
            LOS_TM_ID[packnum] = nID_GNSS;
            LOS_TM_Type[packnum] = 0; 
            break;

            case 16:  //高分相机
            LOS_TM_ID[packnum] = nID_CIOMPCam;
            LOS_TM_Type[packnum] = 0; 
            break;

            case 18:  //小相机
            LOS_TM_ID[packnum] = nID_CMOS;
            LOS_TM_Type[packnum] = 0; 
            break;

            case 24:  //没有开机的或者到头了
            if (packsel == 0)
            {
                LOS_TM_ID[packnum] = nID_AOCS;
                LOS_TM_Type[packnum] = 0; 
                packsel++;
            }
            else if (packsel == 1)
            {
                LOS_TM_ID[packnum] = nID_AOCS;
                LOS_TM_Type[packnum] = 1; 
                packsel++;
            }
            else if (packsel == 2)
            {
                LOS_TM_ID[packnum] = nID_PD;
                LOS_TM_Type[packnum] = 0; 
                packsel++;
            }
            else if (packsel == 3)
            {
                LOS_TM_ID[packnum] = nID_TC;
                LOS_TM_Type[packnum] = 0; 
                packsel++;
            }
            else if (packsel == 4)
            {
                LOS_TM_ID[packnum] = nID_TTCBoard;
                LOS_TM_Type[packnum] = 0; 
                packsel++;
            }
            else if (packsel == 5)
            {
                LOS_TM_ID[packnum] = nID_OBC;
                LOS_TM_Type[packnum] = 4; 
                packsel++;
            }
            else if (packsel == 6)
            {
                LOS_TM_ID[packnum] = nID_OBC;
                LOS_TM_Type[packnum] = 5; 
                packsel = 0;
            }
            break;

            default:
            if (packsel == 0)
            {
                LOS_TM_ID[packnum] = nID_AOCS;
                LOS_TM_Type[packnum] = 0; 
                packsel++;
            }
            else if (packsel == 1)
            {
                LOS_TM_ID[packnum] = nID_AOCS;
                LOS_TM_Type[packnum] = 1; 
                packsel++;
            }
            else if (packsel == 2)
            {
                LOS_TM_ID[packnum] = nID_PD;
                LOS_TM_Type[packnum] = 0; 
                packsel++;
            }
            else if (packsel == 3)
            {
                LOS_TM_ID[packnum] = nID_TC;
                LOS_TM_Type[packnum] = 0; 
                packsel++;
            }
            else if (packsel == 4)
            {
                LOS_TM_ID[packnum] = nID_TTCBoard;
                LOS_TM_Type[packnum] = 0; 
                packsel++;
            }
            else if (packsel == 5)
            {
                LOS_TM_ID[packnum] = nID_OBC;
                LOS_TM_Type[packnum] = 4; 
                packsel++;
            }
            else if (packsel == 6)
            {
                LOS_TM_ID[packnum] = nID_OBC;
                LOS_TM_Type[packnum] = 5; 
                packsel = 0;
            }
        }
    }
}

/* 上电单机检测函数 */
uint8_t LOS_TM_GetPDPWRON()
{
    static uint8_t cnt = 0;
    while(cnt < 24)
    {
        if (STA_EPS.SWITCH_PD[cnt] == 0xAA)
        {
            if (cnt == 0 || cnt == 1 || cnt == 2 || cnt == 7 || cnt == 16 || cnt == 18)
            {
                cnt++;
                return cnt-1;
            }
        }
        cnt++;
    }
    cnt = 0;
    return 24;
}

